#include "PID.h"


PID JY62_PID;
PID sensor_track_PID;
PID Speed_PID;

void PID_Init(PID *pPID)
{
    pPID->SetValue=0.0f;    // 期望值设置为0
    pPID->pid_Calc_out=0.0f;//PID控制器输出设置为0
    pPID->error=0.0f;//当前偏差值设置为0
    pPID->error_last=0.0f;//上次偏差设置为0
    pPID->error_pre=0.0f;//上上次偏差设置为0
    pPID->integral=0.0f;//积分值设置为0
    pPID->Kp=0.0f;
    pPID->Ki=0.0f;
    pPID->Kd=0.0f;
}


float PID_Calc(PID *pPID_Calc, float Next_val, float actual, uint8_t flag) // 位置式PID运算核心
{
    pPID_Calc->SetValue = Next_val;
		
	
    // 计算原始误差
    pPID_Calc->error = pPID_Calc->SetValue - actual; //目标值减去实际值 == 误差值

    // 关键修改点：处理角度跳变问题（当flag=1时）
    if(flag == 1)
    {
        // 将误差映射到[-180, 180)区间
        if(pPID_Calc->error >= 180.0f)  pPID_Calc->error -= 360.0f;
        if(pPID_Calc->error <  -180.0f) pPID_Calc->error += 360.0f;
    }

    // --- 原有积分处理保持不变 ---
    float integral_max = pPID_Calc->integral_max; // 使用结构体中的积分最大值
    float integral_min = -pPID_Calc->integral_max; // 使用结构体中的积分最小值
    pPID_Calc->integral += pPID_Calc->error;

    if(pPID_Calc->integral > integral_max) {
        pPID_Calc->integral = integral_max;
    } else if(pPID_Calc->integral < integral_min) {
        pPID_Calc->integral = integral_min;
    }

    // --- 原有PID计算保持不变 ---
    pPID_Calc->pid_Calc_out = (pPID_Calc->Kp * pPID_Calc->error) 
                             + (pPID_Calc->Ki * pPID_Calc->integral)
                             + pPID_Calc->Kd * (pPID_Calc->error - pPID_Calc->error_last);

    pPID_Calc->error_last = pPID_Calc->error;
//	float derivative = pPID_Calc->Kd * (pPID_Calc->error - pPID_Calc->error_last);
//	printf("Derivative: %f\n", derivative);  // 通过串口观察微分项的值
//	   printf("error: %f, error_last: %f\n", pPID_Calc->error, pPID_Calc->error_last);
//	printf("PID struct addr: %p\n", pPID_Calc);
    return pPID_Calc->pid_Calc_out;
}
//增量式PID
float PID_Calc_Incremental(PID *pid, float actual) 
{
    float error = pid->SetValue - actual;
    
    float delta = pid->Kp * (error - pid->error_last)
                + pid->Ki * error
                + pid->Kd * (error - 2*pid->error_last + pid->error_pre);
    
    pid->error_pre = pid->error_last;
    pid->error_last = error;
    
    return delta;
}








